#include <rov/sensors.h>
#include <rov/server.h>
#include <rov/misc.h>

#include <stdio.h>
#include <pthread.h>


#define BUFFER_SIZE	127
#define IMU_PORT	32001
#define SENSORS_PORT	32002
#define SENSORS_CHAR	's'


int callback(Conn * clients, char * buf, int len)
{
    if (buf[0] != SENSORS_CHAR)
    {
        if (clients[0].fd >= 0)
        {
            int n = sendto(clients[0].fd, buf, len, MSG_NOSIGNAL, (struct sockaddr *) &clients[0].addr, sizeof(clients[0].addr));

fprintf(stderr, "0>");

            if (n < 0)
            {
                fprintf(stderr, "error: imu connection closed\n");
                close(clients[0].fd);
                clients[0].fd = -1;
            }
        }
    } else {
        if (clients[1].fd >= 0)
        {
            int n = sendto(clients[1].fd, buf, len, MSG_NOSIGNAL, (struct sockaddr *) &clients[1].addr, sizeof(clients[1].addr));

fprintf(stderr, "1>");

            if (n < 0)
            {
                fprintf(stderr, "error: sensors connection closed\n");
                close(clients[1].fd);
                clients[1].fd = -1;
            }
        }
    }

buf[len] = 0;
fprintf(stderr, "imu: \"%s\"\n", buf);

    return 0;
}


void * senderThreadEntry(void * conn)
{
    char buf[BUFFER_SIZE + 1];
    Conn * clients = (Conn *) conn;

    int status = loopDataGather<Conn*>(callback, clients[0].sensor, buf, BUFFER_SIZE, '\n', clients);

    if (status != 0)
        fprintf(stderr, "error reading imu data\n");

    return NULL;
}


int main(int argc, char ** argv)
{
    Conn clients[2];

    clients[0].sensor   = clients[1].sensor   = initImu();
    clients[0].fd       = clients[1].fd       = -1;
    clients[0].callback = clients[1].callback = NULL;

    clients[0].port = IMU_PORT;
    clients[1].port = SENSORS_PORT;

    fprintf(stderr, "imu status: %d\n", clients[0].sensor);

    pthread_t imuServer;
    pthread_t sensorServer;

    pthread_create(&imuServer, NULL, serverThreadEntry, &clients[0]);
    pthread_create(&sensorServer, NULL, serverThreadEntry, &clients[1]);

    senderThreadEntry(&clients);

fprintf(stderr, "terminating\n");

    return 0;
}
